#ifndef LINE_HSHAK
#define LINE_HSHAK

#include "Collision_detector.h"
#include <math.h>

class Line
{
public:

	//Defult CTOR
	Line ()
	{}

	Line (Collision_detector* col,Point p,double step):
	l_collision(col),l_c(p),l_step(step),center_raduis(0.000005)
	{
		build_line();
		clean_line();
	}

	//rebuild the line from source point p
	void change_line(Point p)		
	{
		l_c=p;
		build_line();
		clean_line();
	}

	// return random point from the line
	Point get_rand_point()
	{
		if(l_points.size() == 0)
		{//if the line is empty return the source point
			return l_c;
		}
		else
		{
			int index = rand() % l_points.size();
			return l_points[index];
		}
	}

	// return the line size.
	int getSize()
	{
		return l_points.size();
	}

	// check if the line contain point p
	bool contain(Point p)
	{
		return std::find(l_points.begin(),l_points.end(),p) != l_points.end();
	}

private:
	
	Collision_detector* l_collision;
	
	std::vector<Point> l_points;	// the point in the line
	
	Point l_c;						// the source point
	
	double l_step;					// the rate of the point in the line

	double center_raduis;			// delta around the source point
	
	// remove the points that collide with the obstacles
	void clean_line()				
	{
		bool collide = false;

		int l_right_point = 0;		// the right bound of the source point

		int l_left_point = 0;		// the left bound of the source point
	
		double curr_p , next_p;

		double mid_p = l_c[0];		//x the main coordinate of the source
		int vertical  = 0;

		if(l_c[2] == 0.25 || l_c[2] == 0.75)
		{//y
			vertical = 1;
			mid_p = l_c[1];
		}
		//search for the source point index
		for (int i = 0; i < l_points.size()-1;i++)
		{
			curr_p = l_points[i][vertical];
			next_p = l_points[i+1][vertical];

			if(curr_p<mid_p && next_p>mid_p)
			{//source point between next and curr
				l_left_point = i;
				l_right_point = i+1;
				break;
			}
			else if(i==0 && mid_p<curr_p)
			{//source point int the satrt of the line
				l_right_point = i;
				l_left_point = i;
				break;
			}
			else if(i == l_points.size()-2 && mid_p > next_p)
			{//source point in  the end of the line
				l_right_point = i+1;
				l_left_point = i+1;
				break;
			}
		}
		//clean the line right from the source point
		for (int i = l_right_point; i < l_points.size();i++)
		{
			if(collide || !(l_collision->valid(l_points[i])))
			{//if we reach collistion we remove every point from this point
				collide = true;
				l_points.erase(l_points.begin()+i);
				i--;
			}
		}
		collide=false;
		//clean the line left from the source point
		for (int i = l_left_point; i >= 0; i--)
		{
			if(collide || !(l_collision->valid(l_points[i])))
			{//if we reach collistion we remove every point from this point
				collide = true;
				l_points.erase(l_points.begin()+i);
			}
		}
	}
	
	// counstruct the line
	void build_line()				
	{
		//need to change how the robot move TODOs
		double left_p = -1;			//the left limit of workspace
		double curr_p = left_p+l_step;
		double right_p = 1;			//the right limit of workspace
		bool skip_source=true;		// if the source point is part of the line we skip it
	
		double mid_p = l_c[0]; //x the main coordinate of the source

		if(l_c[2] == 0.25 || l_c[2] == 0.75)
		{//y
			mid_p = l_c[1];
		}

		while (curr_p < right_p)
		{
			if(skip_source && fabs(curr_p-mid_p) < center_raduis)
			{//if the current point is a sample point
				skip_source = false;
				curr_p+=l_step;

				if(curr_p + l_step >= right_p)
				{//and the next point is outside the workspace
					break;
				}
			}
			Point p = line_eq(curr_p);
			if((p[0] <=1) && (p[0] >= -1) && (p[1] <= 1) && (p[1] >= -1))
			{
				l_points.push_back(p);
			}
			curr_p+=l_step;
		}
	}

	//Return the image of x
	Point line_eq(double x)		
	{	
		Point p;

		if(l_c[2] == 0.25 || l_c[2] == 0.75)
		{// if the line is vertical
			p = Point(l_c[0], x , l_c[2]); 
		}
		else
		{// y-y0 = m(x-x0)
			p = Point(x , tan(l_c[2]*2*PI)*(x-l_c[0])+l_c[1] , l_c[2]);
		}

		return p;
	}
};
#endif //LINE_H